﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using KZ400B.Link;

namespace KZ400B
{
    class oCommLockedException : Exception
    {
        public override string Message
        {
            get
            {
                return "通讯信道被占用";
            }
        } 
    }

    public class oAxleManager
    {
        private static SerialPortLink spLink = new SerialPortLink();
        //private static Dictionary<string, Axle> s_axleList = new Dictionary<string, Axle>();
        //private static Dictionary<string, Axle> s_axleByIdList = new Dictionary<string, Axle>();
        //private static int s_axleId = 1;
        private static object s_spLocker = new object();
        private static object s_spflgLocker = new object();
        private static bool s_isLocked = false;
        public static event AxlePositionChange OnAxlePostionChange;
        public static event AxleActionResponse OnAxleActionResponse;

        private static Dictionary<string, bool> m_pChanged = new Dictionary<string, bool>();
        public static void setParameterChanged(string axleName, bool chg)
        {
            if (!m_pChanged.ContainsKey(axleName))
            {
                m_pChanged.Add(axleName, chg);
            }
            else
            {
                m_pChanged[axleName] = chg;
            }
        }

        public static bool ParameterChanged(string axleName)
        {
            if (!m_pChanged.ContainsKey(axleName))
            {
                return true;
            }
            else
            {
                return m_pChanged[axleName];
            }
        }


        #region 自动查询位置
        private static object s_tQPLocker = new object();//自动接收线程锁
        private static bool s_autoQP = false;//是否需要自动接收(运行、停止、归零时需要)
        private static Thread t_AutoQP = new Thread(new ThreadStart(autoQP));//自动查询位置
        private static bool isAutoQP()
        {
            lock (s_tQPLocker)
            {
                return s_autoQP;
            }
        }

        private static void setAutoQP(bool auto)
        {
            lock (s_tQPLocker)
            {
                s_autoQP = auto ;
            }
        }
        private static void autoQP()
        {
            int interval = 1000 * 2;
            while (isSysRunning() )
            {
                if (!isAutoQP())
                {
                    Thread.Sleep(100);
                    continue;
                }
                int counter = interval;
                if (counter >= interval)
                {
                    counter = 0;
                    List<string> rList = new List<string>( getRunningAxleList());
                    if (rList != null)
                    {
                        foreach (string an in rList)
                        {
                            QueryPosition(an);
                            if (allAxleStoped())
                            {
                                break;
                            }
                        }
                    }
                }
                else
                {
                    counter += 100;
                    Thread.Sleep(100);
                }
            }
        }
        #endregion

        #region 自动接收线程
        private static object s_tLocker = new object();//自动接收线程锁
        private static bool s_rFlag = false;//系统是否运行
        private static bool s_tStarted = false;//线程是否已启动
        private static bool s_autoRecv = false;//是否需要自动接收(运行、停止、归零时需要)
        private static Thread t_AutoRecvThread = new Thread(new ThreadStart(autoRecv));//自动接收线程
        private static bool isSysRunning()
        {
            lock (s_tLocker)
            {
                return s_rFlag;
            }
        }
        private static void setSysRunning(bool flag)
        {
            lock (s_tLocker)
            {
                 s_rFlag = flag;
            }
        }

        private static bool isAutoRecvStarted()
        {
            lock (s_tLocker)
            {
                return s_tStarted;
            }
        }
        private static void setAutoRecvStarted(bool flag)
        {
            lock (s_tLocker)
            {
                s_tStarted = flag;
            }
        }

        private static bool isAutoRecv()
        {
            lock (s_tLocker)
            {
                return s_autoRecv;
            }
        }
        private static void setAutoRecv(bool flag)
        {
            lock (s_tLocker)
            {
                s_autoRecv = flag;
            }
        }

        /// <summary>
        /// 自动接收线程
        /// </summary>
        private static void autoRecv()
        {
            while (isSysRunning())
            {
                if (isAutoRecv())
                {
                    //处理接收
                    if (!IsLocked)
                    {
                        setIsLocked(true);
                        lock (s_spLocker)
                        {
                            if (spLink.CanRead)
                            {
                                string rdString = spLink.ReadString();
                                if (!string.IsNullOrEmpty(rdString))
                                {
                                    try
                                    {
                                        AxleAction action = Protocol.UnpackAction(rdString);
                                        if (action.Result )
                                        {
                                            foreach (string axle in action.AxleList)
                                            {
                                                if (OnAxleActionResponse != null)
                                                {
                                                    OnAxleActionResponse(axle
                                                        , AxleCacheManager.getAxle(axle)
                                                        , action.Action
                                                        , action.Result
                                                        , string.Empty
                                                        );
                                                }
                                            }
                                        }
                                    }
                                    catch (Exception ex)
                                    {
                                        Console.WriteLine("自动接收动作异常:{0}", ex.Message);
                                    }
                                }
                            }
                        }
                        setIsLocked(false);
                    }
                    
                }
                else
                {
                    Thread.Sleep(100);
                }
            }
        }
        #endregion

        public static bool IsLocked
        {
            get
            {
                lock (s_spflgLocker)
                { return s_isLocked; }
            }
        }

        private static void setIsLocked(bool isLocked)
        {
            lock (s_spflgLocker)
            {  s_isLocked = isLocked; }
        }

        //public static void AddAxle(Axle axle)
        //{
        //    if (!s_axleList.ContainsKey(axle.Name))
        //    {
        //        s_axleList.Add(axle.Name, axle);
        //        s_axleByIdList.Add(s_axleId.ToString(), axle);
        //        s_axleId++;
        //    }
        //}

        public static bool IsOpen
        {
            get { return spLink.IsOpen; }
        }
        public static bool Open(SPort sPort)
        {
            setSysRunning(true);
            
            bool rst = spLink.Open(sPort);
            if (rst)
            {
                rst = Contact();

            }
            if (OnAxleActionResponse != null)
            {
                OnAxleActionResponse(
                    Axle.AllAxleName
                    , new Axle(Axle.AllAxleName)
                    , AxleActions.ContactRespond
                    , rst
                    , string.Format("联系指令:{0}",rst ?"成功":"失败")
                    );

            }
            /*
            if (rst && !isAutoRecvStarted())
            {
                setAutoRecvStarted(true);
                try
                {
                    if (t_AutoRecvThread.ThreadState != ThreadState.Running)
                    {
                        t_AutoRecvThread.Start();
                    }
                }
                catch (Exception ex)
                {
                    Utility.FileOperate.WriteToLog(string.Format("t_AutoRecvThread error:{0}",ex.Message));  
                }
                try
                {
                    if (t_AutoQP.ThreadState != ThreadState.Running)
                    {
                        t_AutoQP = new Thread(new ThreadStart(autoQP));//自动查询位置
                        t_AutoQP.Start();
                    }
                }
                catch (Exception ex)
                {
                    Utility.FileOperate.WriteToLog(string.Format("t_AutoQP error:{0}", ex.Message));
                }
                
            }
             */ 
            return rst;
        }

        public static void Close()
        {
            setSysRunning(false);
            setAutoRecvStarted(false);
            spLink.Close();
            GC.WaitForPendingFinalizers();
            //if (t_AutoRecvThread != null && t_AutoRecvThread.ThreadState == ThreadState.Running)
            //{
            //    t_AutoRecvThread.Join();
            //}
            //t_AutoRecvThread = null;
        }

        public static bool normalSingleResponse(string repString)
        {
            bool rst = false;
            if (!string.IsNullOrEmpty(repString))
            {
                string tmpString = repString.TrimEnd(' ').TrimEnd(CommFlag.AllEnd[0]);

                string okString = Protocol.getACKString();
                string errString = Protocol.getNACKString();

                if (tmpString.StartsWith(okString))
                {
                    rst = true;
                }
                else if (tmpString.StartsWith(errString))
                {
                    //todo:error happen
                }

            //    StringBuilder sComm = new StringBuilder();
            //    byte vCode1 = 0;
            //    byte vCode2 = 0;
            //    if (tmpString.Length > 2)
            //    {
            //        for (int i = 0; i < tmpString.Length - 2; i++)
            //        {
            //            sComm.Append(tmpString[i]);
            //        }
            //        vCode1 = (byte)tmpString[tmpString.Length-2];
            //        vCode2 = (byte)tmpString[tmpString.Length-1];
            //    }
            //    string commString = sComm.ToString();
            //    if (Verify.IsVerify(commString, vCode1, vCode2))
            //    {
            //        string okString = Protocol.getACKString();
            //        string errString = Protocol.getNACKString();
            //        if (commString.Equals(okString))
            //        {
            //            rst = true;
            //        }
            //        else if (commString.Equals(errString))
            //        {
            //            //todo:error happen
            //        }
            //    }
            }
            return rst;
        }

        static bool waitLock()
        {
            if (isSysRunning() )
            {
                if (IsLocked)
                {
                    return false;
                }
                else
                {
                    return true;
                }
            }
            return false;
        }

        //static bool waitLock(int timeout)
        //{
        //    int it = 0;
        //    while ( isSysRunning() && (it < timeout || timeout == 0))
        //    {
        //        if (IsLocked)
        //        {
        //            it += 10;
        //            Thread.Sleep(10);
        //        }
        //        else
        //        {
        //            return true;
        //        }
        //    }
        //    return false;
        //}

        /// <summary>
        /// 收到"=OK"联系成功
        /// </summary>
        /// <returns></returns>
        public static bool Contact()
        {
            if (!IsOpen)
            {
                return false;
            }
            //if (!waitLock(1000))
            //{
            //    throw new CommLockedException();
            //}
            //setIsLocked(true);
            if (!waitLock())
            {
                return false;
            }
            string data = Protocol.Packet( Protocol.GetContactString() );

            try
            {
                lock (s_spLocker)
                {
                    if (spLink.CanWrite)
                    {
                        spLink.Write(data);
                        if (spLink.CanRead)
                        {
                            string rdString = spLink.ReadString();
                            return normalSingleResponse(rdString);
                        }
                    }
                }
            }
            finally
            {
                //setIsLocked(false);
            }
            
            return false;
        }
        private static Dictionary<string,double> m_rSteps = new Dictionary<string,double>();

        private static double getRStep(Axle.Direction dir
            , double totalStep)
        {
            double rS = 0;
            if (dir.Value == Axle.Direction.ForwardDirection.Value)
            {
                rS = totalStep;
            }
            else
            {
                rS = 0 - totalStep;
            }
            return rS;
        }
        /// <summary>
        /// 基本运行参数设置
        /// </summary>
        /// <param name="axleName"></param>
        /// <param name="speed"></param>
        /// <param name="dir"></param>
        /// <param name="totalStep"></param>
        /// <returns></returns>
        public static bool ParameterSetting(
            string axleName
            , Axle.Direction dir
            , double period
            , double totalStep   
         )
        {
            double nStep = getRStep(dir,totalStep);
            if (!m_rSteps.ContainsKey(axleName))
            {
                m_rSteps.Add(axleName, 0.0F);
            }
            if (!ParameterChanged(axleName) && m_rSteps[axleName].ToString("#.000").Equals(nStep.ToString("#.000")))
            {
                return true;
            }

            if (!IsOpen)
            {
                setParameterChanged(axleName, false);
                return false;
            }

            if (!waitLock())
            {
                setParameterChanged(axleName, false);
                return false;
            }

            //if (!waitLock(1000))
            //{
            //    throw new CommLockedException();
            //}
            //setIsLocked(true);
            string data = Protocol.Packet(
                Protocol.GetParameterSettingString(axleName,dir,period,totalStep)
                );
            try
            {
                lock (s_spLocker)
                {
                    if (spLink.CanWrite)
                    {
                        spLink.Write(data);
                        if (spLink.CanRead)
                        {
                            string rdString = spLink.ReadString();
                            
                            bool rst = normalSingleResponse(rdString);
                            if (rst)
                            {
                                m_rSteps[axleName] = nStep;
                            }
                            return rst;
                        }
                    }
                }//lock(s_spLocker)
            }
            finally
            {
                setParameterChanged(axleName, false);
                //setIsLocked(false);
            }
            return false;
        }

        /// <summary>
        /// 加速度参数设置
        /// </summary>
        /// <param name="axleName"></param>
        /// <param name="aLevel"></param>
        /// <returns></returns>
        public static bool AccelerationSetting(
        string axleName
        , Axle.AccelerationLevel aLevel
        )
        {
            if (!IsOpen)
            {
                return false;
            }
            if (!waitLock())
            {
                return false;
            }

            //if (!waitLock(1000))
            //{
            //    throw new CommLockedException();
            //}
            //setIsLocked(true);


            string data = Protocol.Packet(
                Protocol.GetAccelerationSettingString(axleName, aLevel)
                );
            try
            {
                lock (s_spLocker)
                {
                    if (spLink.CanWrite)
                    {
                        spLink.Write(data);
                        if (spLink.CanRead)
                        {
                            string rdString = spLink.ReadString();
                            return normalSingleResponse(rdString);
                        }
                    }
                }//lock
            }
            finally
            {
                //setIsLocked(false);
            }
            return false;
        }

        private static List<string> s_runningAxleList;
        private static object s_raLocker = new object();
        private static void setRunningAxleList(List<string> axleList)
        {
            lock (s_raLocker)
            {
                s_runningAxleList = axleList;
            }
        }

        private static List<string> getRunningAxleList()
        {
            lock (s_raLocker)
            {
                return s_runningAxleList;
            }
        }

        private static void removeRunningAxle(string axle)
        {
            lock (s_raLocker)
            {
                if (s_runningAxleList != null && s_runningAxleList.Contains(axle))
                {
                    s_runningAxleList.Remove(axle);
                }
            }
        }

        private static bool axleIsRunning(string axle)
        {
            if (allAxleStoped())
                return false;
            lock (s_raLocker)
            {
                if (s_runningAxleList != null && s_runningAxleList.Contains(axle))
                {
                    return true;
                }
            }
            return false;

        }

        private static bool allAxleStoped()
        {
            lock (s_raLocker)
            {
                return s_runningAxleList == null || s_runningAxleList.Count == 0;
            }
        }


        /// <summary>
        /// 运行
        /// </summary>
        /// <param name="axleName"></param>
        /// <returns></returns>
        public static void Run(string axleName)
        {
            if (!IsOpen)
            {
                return;
            }
            //if (!waitLock(1000))
            //{
            //    throw new CommLockedException();
            //}
            List<string> axleNameList = new List<string>();
            axleNameList.Add(axleName);
            Run(axleNameList);
        }

        /// <summary>
        /// 运行
        /// </summary>
        /// <param name="axleNameList"></param>
        /// <returns></returns>
        public static void Run(List<string> axleNameList)
        {
            if (!IsOpen)
            {
                return;
            }
            Thread t_tRun = new Thread(new ParameterizedThreadStart(run));
            t_tRun.Start(axleNameList);
            setRunningAxleList(axleNameList);
        }


        /// <summary>
        /// 运行
        /// </summary>
        /// <param name="axleNameList"></param>
        /// <returns></returns>
        public static void run(object arg)
        {
            if (!IsOpen)
            {
                return;
            }
            List<string> axleNameList = arg as List<string>;
#if !DEBUG
            if (!waitLock())
            {
                return;
            }
            //if (!waitLock(1000))
            //{
            //    throw new CommLockedException();
            //}
            setIsLocked(true);
#endif

            string data = Protocol.Packet(
                Protocol.GetRunString(axleNameList)
                );
            Dictionary<string, string> axleIdNameDict = new Dictionary<string, string>();
            int aId = 1;
            foreach (string aName in axleNameList)
            {
                axleIdNameDict.Add(aId.ToString(), aName);
                aId++;
            }
            try
            {
                List<string> rQList = null;
                bool eRst = false;
                lock (s_spLocker)
                {
                    if (spLink.CanWrite)
                    {
                        lock (s_spLocker)
                        {
                            spLink.Write(data);
                            if (spLink.CanRead)
                            {
                                string rdString = spLink.ReadString();
                                eRst = normalSingleResponse(rdString);


                                if (OnAxleActionResponse != null)
                                {
                                    OnAxleActionResponse(Axle.AllAxleName
                                        , AxleCacheManager.getAxle(Axle.AllAxleName)
                                        , AxleActions.Run
                                        , eRst
                                        , string.Format("[{0}] 轴 running Response : {1} "
                                            , Axle.AllAxleName, eRst.ToString()
                                        )
                                    );
                                }



                            }
                        }
                    }
                }//link lock


                if (eRst)
                {
                    if (OnAxleActionResponse != null)
                    {
                        OnAxleActionResponse(Axle.AllAxleName
                            , AxleCacheManager.getAxle(Axle.AllAxleName)
                            , AxleActions.Run
                            , eRst
                            , string.Format("[{0}] 轴 running executing : {1} "
                                , Axle.AllAxleName, eRst.ToString()
                            )
                        );
                    }

                    rQList = new List<string>(getRunningAxleList());

                    string ackString = Protocol.getACKString();
                    //setAutoQP(true);//开启自动查询位置线程取得最新位置信息
                    //comm success
                    //need check result
                    bool allDone = false;
                    while (!allDone)
                    {
                        //连接断开
                        if (!isSysRunning())
                        {
#if !DEBUG
                            setIsLocked(false);
#endif
                            return;
                        }
                        if ( bestoping())
                        {
                            allDone = allAxleStoped() || axleIdNameDict.Count == 0;
                            if (allDone)
                            {
                                break;
                            }
                            Thread.Sleep(500);
                            continue;
                        }

                        if (spLink.CanRead)
                        {
                            string nrdString = string.Empty;
                            nrdString = spLink.ReadString(true);
                            if (string.IsNullOrEmpty(nrdString))
                            {
                                allDone = allAxleStoped() || axleIdNameDict.Count == 0;
                                if (allDone)
                                {
                                    break;
                                }
                                Thread.Sleep(200);
                                continue;
                            }
                            AxleAction action = Protocol.UnpackAction(nrdString);
                            if (action.Result /*&& OnAxleActionResponse != null*/)
                            {
                                foreach (string axle in action.AxleList)
                                {
                                    axleIdNameDict.Remove(axle);
                                    removeRunningAxle(axle);
                                    if (OnAxleActionResponse != null)
                                    {
                                        OnAxleActionResponse(axle
                                            , AxleCacheManager.getAxle(axle)
                                            , AxleActions.Run
                                            , action.Result
                                            , string.Format("[{0}] 轴 running completed : {1} "
                                                , axle, action.Result.ToString()
                                            )
                                        );
                                    }
                                    //在电机运行结束后再读取位置信息
                                }
                            }
                            else
                            {
                                if (OnAxleActionResponse != null)
                                {
                                    OnAxleActionResponse(Axle.AllAxleName
                                        , AxleCacheManager.getAxle(Axle.AllAxleName)
                                        , AxleActions.Run
                                        , action.Result
                                        , string.Format(" running completed : {0} "
                                            , "收到错误返回码"
                                        )
                                    );
                                }
                                break;
                            }
                        }
                        allDone = allAxleStoped() || axleIdNameDict.Count == 0;
                    }

                    
                    //在电机运行结束后再读取位置信息
                    if (rQList != null)
                    {
                        foreach (string an in rQList)
                        {
                            if (OnAxleActionResponse != null)
                            {
                                OnAxleActionResponse(an
                                    , AxleCacheManager.getAxle(an)
                                    , AxleActions.Executing
                                    , true
                                    , string.Format(" 正在读取[{0}]轴位置信息.... "
                                        , an
                                    )
                                );
                            }
                            QueryPosition(an);
                        }
                    }
                    

                    if (OnAxleActionResponse != null)
                    {
                        OnAxleActionResponse(Axle.AllAxleName
                            , AxleCacheManager.getAxle(Axle.AllAxleName)
                            , AxleActions.Executing
                            , true
                            , string.Format(" 运行结束. ")
                        );
                    }

                    //setAutoQP(false);//关闭自动查询位置线程取得最新位置信息
                    //setAutoRecv(true);//开启自动接收线程判断是否执行成功
                    //setAutoQP(true);//开启自动查询位置线程取得最新位置信息
                    return;//true;

                }
            }
            catch (Exception ex)
            {
                if (OnAxleActionResponse != null)
                {
                    OnAxleActionResponse(Axle.AllAxleName
                        , AxleCacheManager.getAxle(Axle.AllAxleName)
                        , AxleActions.Exception
                        , false
                        , string.Format("[{0}] 轴 running exception : {1} "
                            , Axle.AllAxleName, ex.Message
                        )
                    );
                }
            }
            finally
            { 
#if !DEBUG
                setIsLocked(false);
#endif
            }

            //return false;
        }

        /// <summary>
        /// 停止
        /// </summary>
        /// <param name="axleName"></param>
        /// <returns></returns>
        public static void Stop(string axleName)
        {
            if (!IsOpen)
            {
                return;// false;
            }
            //if (!waitLock())
            //{
            //    return true;
            //}

            //if (!axleIsRunning(axleName))
            //{
            //    return;// true;
            //}
            //if (!waitLock(1000))
            //{
            //    throw new CommLockedException();
            //}
            

            List<string> axleNameList = new List<string>();
            axleNameList.Add(axleName);
             Stop(axleNameList);
        }

        public static void Stop(List<string> axleNameList)
        {
            Thread t_tStop = new Thread(new ParameterizedThreadStart(stop));
            t_tStop.Start(axleNameList);
        }
        static object m_stopLocker = new object();
        static bool m_bstop = false;
        static bool bestoping()
        {
            lock (m_stopLocker)
            {
                return m_bstop;
            }
        }
        static void setstoping(bool stoping)
        {
            lock (m_stopLocker)
            {
                m_bstop = stoping;
            }
        }

        /// <summary>
        /// 
        /// 停止
        /// </summary>
        /// <param name="axleNameList"></param>
        /// <returns></returns>
        public static void stop(
        object arg
        )
        {
            if (!IsOpen)
            {
                return;// false;
            }
            List<string> axleNameList = arg as List<string>;
            //if (!waitLock())
            //{
            //    return true;
            //}
            //if (!waitLock(1000))
            //{
            //    throw new CommLockedException();
            //}
            //setIsLocked(true);

            string data = Protocol.Packet(
                Protocol.GetStopString(axleNameList)
                );
            Dictionary<string, string> axleIdNameDict = new Dictionary<string, string>();
            int aId = 1;
            foreach (string aName in axleNameList)
            {
                axleIdNameDict.Add(aId.ToString(), aName);
                aId++;
            }
            try
            {
                bool eRst = false;
                lock (s_spLocker)
                {
                    if (spLink.CanWrite)
                    {
                        setstoping(true);
                        spLink.Write(data);
                        if (spLink.CanRead)
                        {
                            string rdString = spLink.ReadString();
                            eRst = normalSingleResponse(rdString);


                            if (OnAxleActionResponse != null)
                            {
                                OnAxleActionResponse(Axle.AllAxleName
                                    , AxleCacheManager.getAxle(Axle.AllAxleName)
                                    , AxleActions.Stop
                                    , eRst
                                    , string.Format("[{0}] 轴 stop response : {1} "
                                        , Axle.AllAxleName, eRst.ToString()
                                    )
                                );
                            }

                        }//canwrite
                    }//lock

                    if (eRst)
                    {
                        if (OnAxleActionResponse != null)
                        {
                            OnAxleActionResponse(Axle.AllAxleName
                                , AxleCacheManager.getAxle(Axle.AllAxleName)
                                , AxleActions.Executing
                                , eRst
                                , string.Format("[{0}] 轴 stop executing : {1} "
                                    , Axle.AllAxleName, eRst.ToString()
                                )
                            );
                        }
                    }//eRst
                    else
                    {
                        setstoping(false);
                        return;
                    }

                    string ackString = Protocol.getACKString();
                   

                    bool allDone = false;
                    while (!allDone)
                    {
                        //连接断开
                        if (!isSysRunning())
                        {
#if !DEBUG
                            setIsLocked(false);
#endif
                            return;
                        }
                        if (spLink.CanRead)
                        {
                            string rdString = string.Empty;
                            rdString = spLink.ReadString();
                            AxleAction action = Protocol.UnpackAction(rdString);
                            if (action.Result && OnAxleActionResponse != null)
                            {
                                foreach (string axle in action.AxleList)
                                {
                                    Utility.FileOperate.WriteToLog(
                                        string.Format("Stop Succeed:[{0}|{1}]", axle, action.Result)
                                        );
                                    //axleIdNameDict.Remove(axle);
                                    removeRunningAxle(axle);
                                    if (OnAxleActionResponse != null)
                                    {
                                        OnAxleActionResponse(axle
                                            , AxleCacheManager.getAxle(axle)
                                            , AxleActions.Stop
                                            , action.Result
                                            , string.Format("Stop execute complete:[{0}|{1}]", axle, action.Result)
                                            );

                                    }

                                }
                            }
                        }
                        allDone = allAxleStoped() || axleIdNameDict.Count == 0;
                    }
                    if (allAxleStoped())
                    {
                        setIsLocked(false);
                        setAutoQP(false);
                    }
                    //setAutoRecv(true);//开启自动接收线程判断是否执行成功
                    return;

                    //}

                }
            }
            catch (Exception ex)
            {
                if (OnAxleActionResponse != null)
                {
                    OnAxleActionResponse(Axle.AllAxleName
                        , AxleCacheManager.getAxle(Axle.AllAxleName)
                        , AxleActions.Exception
                        , false
                        , string.Format("[{0}] 轴 stop exception : {1} "
                            , Axle.AllAxleName, ex.Message
                        )
                    );
                }
            }
            finally
            {
                //setIsLocked(false);
                setstoping(false);
            }
            return ;
        }


        /// <summary>
        /// 归零
        /// </summary>
        /// <param name="axleName"></param>
        /// <param name="toZeroSpeed"></param>
        /// <returns></returns>
        public static void ToZero(
       string axleName
       , double period
       )
        {
            Thread t_tozero = new Thread(new ParameterizedThreadStart (toZero));
            t_tozero.Start(new ToZeroArg(axleName,period));
        }


        class ToZeroArg
        {
            public ToZeroArg(    
                string axleName
                , double period
            )
            {
                m_axleName = axleName;
                m_period = period;
            }

            private string m_axleName;

            public string AxleName
            {
                get { return m_axleName; }
                set { m_axleName = value; }
            }
            private double m_period;

            public double Period
            {
                get { return m_period; }
                set { m_period = value; }
            }

        }


        /// <summary>
        /// 归零
        /// </summary>
        /// <param name="axleName"></param>
        /// <param name="toZeroSpeed"></param>
        /// <returns></returns>
        private static void toZero(object arg)
        {
            ToZeroArg tzArg = arg as ToZeroArg;
            string axleName = tzArg.AxleName;
            double period = tzArg.Period;
            if (!IsOpen)
            {
                return; // false;
            }

#if !DEBUG
            if (!waitLock())
            {
                return;
            }
            //if (!waitLock(1000))
            //{
            //    throw new CommLockedException();
            //}
            setIsLocked(true);
#endif

            string data = Protocol.Packet(
                Protocol.GetToZeroString(axleName, period)
                );
            try
            {
                bool rst = false;
                lock (s_spLocker)
                {
                    if (spLink.CanWrite)
                    {
                        spLink.Write(data);
                        if (spLink.CanRead)
                        {
                            string rdString = spLink.ReadString();
                            rst = normalSingleResponse(rdString);


                            if (OnAxleActionResponse != null)
                            {
                                OnAxleActionResponse(axleName
                                    , AxleCacheManager.getAxle(axleName)
                                    , AxleActions.ToZero
                                    , rst
                                    , string.Format("[{0}] 轴 tozero response : {1} "
                                        , axleName, rst.ToString()
                                    )
                                );
                            }
                        }//can read
                    }//can write
                }//lock

                if (rst)
                {
                    if (OnAxleActionResponse != null)
                    {
                        OnAxleActionResponse(axleName
                            , AxleCacheManager.getAxle(axleName)
                            , AxleActions.ToZero
                            , rst
                            , string.Format("[{0}] 轴 tozero executing : {1} "
                                , axleName, rst.ToString()
                            )
                        );
                    }

                    string tString = string.Empty;
                    bool allDone = false;
                    while (!allDone)
                    {
                        //连接断开
                        if (!isSysRunning())
                        {
#if !DEBUG
                            setIsLocked(false);
#endif
                            return;
                        }
                        if (bestoping())
                        {
                            allDone = allAxleStoped();
                            if (allDone)
                            {
                                break;
                            }
                            Thread.Sleep(500);
                            continue;
                        }

                        if (spLink.CanRead)
                        {
                            string nrdString = string.Empty;
                            nrdString = spLink.ReadString(true);
                            if (string.IsNullOrEmpty(nrdString))
                            {
                                //allDone = allAxleStoped();
                                //if (allDone)
                                //{
                                //    break;
                                //}
                                Thread.Sleep(200);
                                continue;
                            }

                            //tString = spLink.ReadString(true);

                            AxleAction action = Protocol.UnpackAction(nrdString);
                            if (action.Result)
                            {
                                foreach (string axle in action.AxleList)
                                {
                                    if (action.Result)
                                    {
                                        Utility.FileOperate.WriteToLog(
                                            string.Format("ToZero Succeed:[{0}|{1}]", axle, action.Result)
                                            );
                                        if (OnAxleActionResponse != null)
                                        {
                                            OnAxleActionResponse(axle
                                                , AxleCacheManager.getAxle(axle)
                                                , AxleActions.ToZeroFinish
                                                , action.Result
                                                , string.Format("ToZero execute completed:[{0}|{1}]", axle, action.Result)
                                                );

                                        }
                                        allDone = true;
                                        break;
                                    }
                                    else
                                    {
                                        Utility.FileOperate.WriteToLog(
                                        string.Format("ToZero Error:[{0}|{1}]", axle, action.Result)
                                        );
                                    }
                                }
                            }
                            if (!string.IsNullOrEmpty(tString))
                            {
                                break;
                            }
                        }
                    }
                }//ack response 

                QueryPosition(axleName);

            }
            catch (Exception ex)
            {
                if (OnAxleActionResponse != null)
                {
                    OnAxleActionResponse(Axle.AllAxleName
                        , AxleCacheManager.getAxle(Axle.AllAxleName)
                        , AxleActions.Exception
                        , false
                        , string.Format("[{0}] 轴 tozero exception : {1} "
                            , Axle.AllAxleName, ex.Message
                        )
                    );
                }
            }
            finally
            {
                #if !DEBUG
                setIsLocked(false);
                #endif
            }

            return;// false;
        }

        /// <summary>
        /// 查询一个轴当前位置
        /// </summary>
        /// <param name="axleName"></param>
        /// <returns></returns>
        public static bool QueryOneAxlePosition(
        string axleName
        ,ref double position
        )
        {
            position = 0.0F;
            if (axleName.Equals(Axle.AllAxleName))
            {
                return false;
            }
            if (!IsOpen)
            {
                return false;
            }
            //if (!waitLock())
            //{
            //    return false;
            //}

            string data = Protocol.Packet(
                Protocol.GetQueryPostionString(axleName)
                );
            try
            {
                
                Axle t_Axle = null;
                lock (s_spLocker)
                {
                    if (spLink.CanWrite)
                    {
                        spLink.Write(data);
                        if (spLink.CanRead)
                        {
                            string rdString = spLink.ReadString();
                            if (!string.IsNullOrEmpty(rdString))
                            {
                                UnpacketResult ur = Protocol.Unpack(rdString);
                                if (ur != null && ur.Result
                                    && ur.CommFlag.Equals(CommFlag.Query)
                                    && !string.IsNullOrEmpty(ur.Content)
                                    )
                                {
                                    string aName = ur.Content[0].ToString();
                                    string aP = ur.Content.Substring(1);
                                    if (!string.IsNullOrEmpty(aName) && !string.IsNullOrEmpty(aP))
                                    {
                                        int aPos = 0;
                                        t_Axle = AxleCacheManager.getAxle(aName);
                                        if (t_Axle != null && int.TryParse(aP, out aPos))
                                        {
                                            double dpos = aPos * t_Axle.OnestepDisplacement;
                                            t_Axle.Position = dpos;
                                            position = dpos;
                                            //
                                            
                                        }
                                    }
                                    rdString = spLink.ReadString();
                                    bool rst = normalSingleResponse(rdString);
                                    if (OnAxlePostionChange != null)
                                    {
                                        OnAxlePostionChange(axleName, t_Axle, position, rst);
                                    }
                                    if (rst)
                                    {
                                        return true;
                                    }
                                    else
                                    {
                                        Utility.FileOperate.WriteToLog("Query {0} Position Error.", axleName);
                                        return false;
                                    }
                                }
                            }
                        }
                    }
                }
            }
            finally
            {
                //setIsLocked(false);
            }

            return false;
        }

        /// <summary>
        /// 查询当前位置
        /// </summary>
        /// <param name="axleName"></param>
        /// <returns></returns>
        public static bool QueryPosition(
        string axleName
        )
        {
            if (!IsOpen)
            {
                return false;
            }
            //if (!waitLock())
            //{
            //    return false;
            //}
            //if (!waitLock(1000))
            //{
            //    throw new CommLockedException();
            //}
            //setIsLocked(true);

            string data = Protocol.Packet(
                Protocol.GetQueryPostionString(axleName)
                );
            try
            {
                lock (s_spLocker)
                {
                    if (spLink.CanWrite)
                    {
                        spLink.Write(data);
                        if (spLink.CanRead)
                        {
                            string rdString = spLink.ReadString();
                            if (!string.IsNullOrEmpty(rdString))
                            {
                                //string okString = Protocol.getACKString();
                                //if (rdString.Equals(okString))
                                //{
                                //    return true;
                                //}
                                //char cSpace = CommFlag.Space[0];
                                //char cAllEnd = CommFlag.AllEnd[0];
                                //char cSingleEnd = CommFlag.SingleEnd[0];
                                //char cSeparate = CommFlag.Separate[0];
                                //if (axleName != Axle.AllAxleName)
                                //{
                                //    string[] xStrings = rdString.TrimEnd(cSpace).TrimEnd(cAllEnd).TrimEnd(cSingleEnd).Split(cSeparate);
                                //    Axle t_Axle = AxleCacheManager.getAxle(axleName);
                                //    if (xStrings != null && t_Axle != null)
                                //    {
                                //        int aPos = 0;
                                //        if (int.TryParse(xStrings[1], out aPos))
                                //        {
                                //            t_Axle.Position = aPos;
                                //            //
                                //            if (OnAxlePostionChange != null)
                                //            {
                                //                OnAxlePostionChange(axleName, t_Axle, aPos);
                                //            }
                                //        }
                                //    }
                                //}
                                //else
                                //{
                                //    string[] xStrings = rdString.TrimEnd(cSpace).TrimEnd(cAllEnd).Split(cSingleEnd);
                                //    if (xStrings != null)
                                //    {
                                //        foreach (string xString in xStrings)
                                //        {
                                //            string[] axleStrings = xString.TrimEnd(cSingleEnd).Split(cSeparate);
                                //            if (axleStrings != null && axleStrings.Length >= 2)
                                //            {
                                //                int aPos = 0;
                                //                Axle t_Axle = AxleCacheManager.getAxleById(axleStrings[0]);
                                //                if (t_Axle != null && int.TryParse(axleStrings[1], out aPos))
                                //                {
                                //                    t_Axle.Position = aPos;
                                //                    //
                                //                    if (OnAxlePostionChange != null)
                                //                    {
                                //                        OnAxlePostionChange(axleName, t_Axle, aPos);
                                //                    }
                                //                }
                                //            }
                                //        }
                                //    }
                                //}
                                UnpacketResult ur = Protocol.Unpack(rdString);
                                if (ur != null && ur.Result
                                    && ur.CommFlag.Equals(CommFlag.Query)
                                    && !string.IsNullOrEmpty(ur.Content)
                                    )
                                {
                                    string aName = ur.Content[0].ToString();
                                    string aP = ur.Content.Substring(1);
                                    Axle t_Axle = null;
                                    double dpos = 0.0F;
                                    if (!string.IsNullOrEmpty(aName) && !string.IsNullOrEmpty(aP))
                                    {
                                        int aPos = 0;
                                        t_Axle = AxleCacheManager.getAxle(aName);
                                        if (t_Axle != null && int.TryParse(aP, out aPos))
                                        {
                                            dpos = aPos * t_Axle.OnestepDisplacement;
                                            t_Axle.Position = dpos;
                                            //
                                            //if (OnAxlePostionChange != null)
                                            //{
                                            //    OnAxlePostionChange(axleName, t_Axle, dpos);
                                            //}
                                        }
                                    }
                                    rdString = spLink.ReadString();
                                    bool rst = normalSingleResponse(rdString);
                                    if (OnAxlePostionChange != null)
                                    {
                                        OnAxlePostionChange(axleName, t_Axle, dpos, rst);
                                    }
                                    if (!rst)
                                    {
                                        Utility.FileOperate.WriteToLog("Query {0} Position Error.", axleName);
                                    }
                                }
                            }
                        }
                    }
                }
            }
            finally
            {
                //setIsLocked(false);
            }

            return false;
        }

        /// <summary>
        /// 查询指令是否成功
        /// </summary>
        /// <returns></returns>
        public static bool QuerySuccess()
        {
            if (!IsOpen)
            {
                return false;
            }
            if (!waitLock())
            {
                return false;
            }
            //if (!waitLock(1000))
            //{
            //    throw new CommLockedException();
            //}
            //setIsLocked(true);

            string data = Protocol.Packet(
                Protocol.GetQuerySuccessString()
                );
            try
            {
                lock (s_spLocker)
                {
                    if (spLink.CanWrite)
                    {
                        spLink.Write(data);
                        if (spLink.CanRead)
                        {
                            string rdString = spLink.ReadString();
                            return normalSingleResponse(rdString);
                        }
                    }
                }//lock
            }
            finally
            {
                //setIsLocked(false);
            }
            return false;
        }


        /// <summary>
        /// 相对零位设置
        /// </summary>
        /// <param name="axleName"></param>
        /// <returns></returns>
        public static bool RelativeZero(string axleName)
        {
            if (!IsOpen)
            {
                return false;
            }
            if (!waitLock())
            {
                return false;
            }
            //if (!waitLock(1000))
            //{
            //    throw new CommLockedException();
            //}
            //setIsLocked(true);
            string data = Protocol.Packet(
                Protocol.GetRelativeZeroString(axleName)
                );
            try
            {
                lock (s_spLocker)
                {
                    if (spLink.CanWrite)
                    {
                        spLink.Write(data);
                        if (spLink.CanRead)
                        {
                            string rdString = spLink.ReadString();
                            bool rst =  normalSingleResponse(rdString);
                            if (rst)
                            {
                                rdString = spLink.ReadString();
                                if (!string.IsNullOrEmpty(rdString))
                                {
                                    UnpacketResult uRst = Protocol.Unpack(rdString);
                                    if (uRst.Result
                                        && uRst.FunctionCode == FunctionCode.RelativeZero
                                        && !string.IsNullOrEmpty(uRst.Content)
                                        )
                                    {
                                        string[] xs = uRst.Content.Split(':');
                                        if (xs != null && xs.Length > 1)
                                        {
                                            if (xs[0] == axleName && xs[1] == "OK")
                                            {
                                                return true;
                                            }
                                        }
                                    }
                                }
                            }//rst
                        }
                    }
                }//lock(s_spLocker)
            }
            finally
            {
                //setIsLocked(false);
            }

            return false;
        }

    }
}
